#include <uhd/usrp/multi_usrp.hpp>
#include <signal.h>
#include <iostream>
#include <stdio.h>
#include <queue>
#include <complex>
#include <thread>
#include <mutex>
#include <cmath>
#include <chrono>
#include "my_usrp.hpp"
#include "main.hpp"
#include "process_data.hpp"

uint64_t symbols_in_buff = 0;
short *buff = new short[SAMPLE_PER_BUFF * 2 * BUFF_SIZE];
MY_USRP::MY_USRP()
{
    ref = "internal";                // clock reference
    clock_rate = 61.44e6;            // clock frequence
    samp_rate = 15.36e6;             // sampling rate
    tx_freq = 2e9;                   // tx center freq
    tx_gain = 90;                    // tx gain
    std::string args = usrp_address; //"serial=1710022";//for usrp address
    usrp = uhd::usrp::multi_usrp::make(args);

    // set clock reference
    usrp->set_clock_source(ref);
    usrp->set_master_clock_rate(clock_rate);
    printf("set the clock rate %0.2f \n", usrp->get_master_clock_rate());
    // set sampling rate
    usrp->set_tx_rate(samp_rate);
    printf("set the tx sample rate to %0.2f \n", usrp->get_tx_rate());
    // set center freq
    usrp->set_tx_freq(tx_freq);
    printf("set the tx center freq to %0.2f \n", usrp->get_tx_freq());
    // set gain
    usrp->set_tx_gain(tx_gain);
    printf("set tx gain to %0.2f \n", tx_gain);
    // set sending flow format
    std::string cpu_format = "sc16";
    std::string wire_format = "sc16";
    uhd::stream_args_t stream_args(cpu_format, wire_format);
    tx_stream = usrp->get_tx_stream(stream_args);

    usrp->set_command_time(md.time_spec);
    aaa = usrptime + 1.5;
    app_start_tick = usrp->get_time_now().to_ticks(clock_rate);
    double time_in_future = 1;
    tx_start_tick = app_start_tick + uhd::time_spec_t(time_in_future).to_ticks(clock_rate);
    tick = tx_start_tick;

    buff = new short[SAMPLE_PER_BUFF * 2];
    if (tx_stream)
    {
        std::cout << "Usrp Init!" << std::endl;
    }
    else
    {
        std::cout << "Usrp Init Failed!" << std::endl;
    }
}

MY_USRP::~MY_USRP()
{
    delete[] buff;
}

void MY_USRP::my_usrp_tx()
{
    static long long int times = 0;
    // static int t = 0;
    // static auto time_stamp = std::chrono::high_resolution_clock::now();
    // std::cout << "sample_per_buff:" << SAMPLE_PER_BUFF << "\n";
    while (!stop_flag || (head_buf != tail_buf))
    {
        if (head_buf == tail_buf)
        {
            // std::cout << "buff empty, waiting" << std::endl;
            continue;
        }
        mtx.lock();
        // t++;
        // std::cout << "send slot " << t << "\n";
        // auto time_temp = std::chrono::high_resolution_clock::now();

        if (first_flag == 0)
        {
            /* In first round send, md.start_of_burst should be set to true*/
            md.has_time_spec = true;
            md.start_of_burst = true;
            md.end_of_burst = false;
            first_flag = 1;
        }
        else
        {
            md.has_time_spec = true;
            md.start_of_burst = false;
            md.end_of_burst = false;
            /*发送数据既不能太快，也不能太慢；需要在上一次发送开始之后，以及上一次发送结束之前，发送出去；
             * 这个循环就是控制PC往USRP送数据的时机；这里是使用的tick这种方式，使用时钟周期计算；
             */
            int64_t current = usrp->get_time_now().to_ticks(clock_rate);
            while (current - app_start_tick < (times - 1) * 1000)
            {
                // fprintf(fpp,"wait time, now gap is %lld,  \n",
                // nowticks - tx_start_tick - uhd::time_spec_t((times - 1) * 0.0005).to_ticks(clk_rate));
                current = usrp->get_time_now().to_ticks(clock_rate);
            }
            // fprintf(fpp,"----------Gap in time is %lf \n", usrp->get_time_now().get_real_secs() - usrptime.get_real_secs());
        }
        /*发送数据之前，指定这组数据应该发送的时刻；使用时钟周期来计算*/
        md.time_spec = uhd::time_spec_t::from_ticks(tick, clock_rate);

        int ret = tx_stream->send(buff, SAMPLE_PER_BUFF, md);
        // std::cout << "send bytes is ret = " << ret << std::endl;
        times++;
        /*计算下一次发送时刻的tick是多少*/
        tick += clock_rate / samp_rate * SAMPLE_PER_BUFF;
        
        // auto duration = std::chrono::duration_cast<std::chrono::microseconds>(time_temp - time_stamp).count();
        // std::cout << "time cost " << duration << std::endl;
        // // if (duration > 500)
        // // {
        // //     std::cout << "time out! " << duration << std::endl;
        // // }
        // time_stamp = time_temp;
        
        tail_buf++;
        tail_buf %= BUFF_SIZE;
        mtx.unlock();
    }
}

// void MY_USRP::my_usrp_tx()
// {
//     static uint16_t processed_symbols = 0; // number of processed symbols in one slot
//     std::cout << "sample_per_buff:" << SAMPLE_PER_BUFF << "\n";
//     while (pre_date_time.size() >= 512)
//     {

//         mtx.lock();
//         // auto start_time_1 = std::chrono::high_resolution_clock::now();
//         if (processed_symbols == 0)
//         {
//             // for (int i = 0; i < 512; i++)
//             // {
//             //     buff[2 * (i + N_cp_first)] = round(pre_date_time.front().real());
//             //     buff[2 * (i + N_cp_first) + 1] = round(pre_date_time.front().imag());
//             //     pre_date_time.pop();
//             // }
//             std::memcpy(buff + 2 * N_cp_first, &pre_date_time.front(), 2 * 512 * sizeof(short));
//             mtx.unlock();
//             for (int i = 0; i < N_cp_first; i++)
//             {
//                 buff[2 * i] = buff[2 * (512 + i)];
//                 buff[2 * i + 1] = buff[2 * (512 + i) + 1];
//             }
//         }
//         else
//         {
//             int temp = (N_cp_first + 512) + (N_cp + 512) * (processed_symbols - 1);
//             // for (int i = 0; i < 512; i++)
//             // {
//             //     buff[2 * (i + N_cp + temp)] = round(pre_date_time.front().real());
//             //     buff[2 * (i + N_cp + temp) + 1] = round(pre_date_time.front().imag());
//             //     pre_date_time.pop();
//             // }
//             std::memcpy(buff + 2 * (N_cp_first + temp), &pre_date_time.front(), 2 * 512 * sizeof(short));
//             mtx.unlock();
//             for (int i = 0; i < N_cp; i++)
//             {
//                 buff[2 * (i + temp)] = buff[2 * (512 + i + temp)];
//                 buff[2 * (i + temp) + 1] = buff[2 * (512 + i + temp) + 1];
//             }
//         }

//         processed_symbols++;
//         static int t = 0;
//         static auto time_stamp = std::chrono::high_resolution_clock::now();
//         if (processed_symbols >= symbol_num)
//         {
//             t++;
//             std::cout << "send slot " << t << "\n";
//             auto time_temp = std::chrono::high_resolution_clock::now();

//             this->tx_stream->send(buff, SAMPLE_PER_BUFF, md);

//             auto duration = std::chrono::duration_cast<std::chrono::microseconds>(time_temp - time_stamp).count();
//             if (duration > 500)
//             {
//                 std::cout << "time out! " << duration << std::endl;
//             }
//             time_stamp = time_temp;

//             processed_symbols = 0;
//         }
//     }
// }
// void MY_USRP::runThread()
// {
//     thread_ = std::thread(&MY_USRP::my_usrp_tx);
//     if (thread_.joinable())
//     {
//         thread_.join();
//     }
// }